function [p_new]=update_p(p_before,p_before2,v_new,v_before,ts)
%%
%用于位置更新
%输入：前一采样时刻位置p_before
%      前二采样时刻位置p_before2
%      当前时刻速度v_new
%      前一时刻速度v_before
%输出：更新位置p_new
%%
% 参数引入
%地球参数设置
%v_ie=7.292e-5; %地球自转角速率
a=6378254;     %地球长半径
e=1/298.3;     %偏心率
%g=9.8;
%%
%开始位置更新
L_before=p_before(1);
%la_before=p_before(2);
h_before=p_before(3);

L_before2=p_before2(1);
%la_before2=p_before2(2);
h_before2=p_before2(3);

L_out=(3*L_before-L_before2)/2;
h_out=(3*h_before-h_before2)/2;

R_m=a/sqrt(1-e^2*sin(L_out));
R_n=a*(1-e^2)/sqrt((1-e^2*sin(L_out)^2)^3);

R_mh_out=R_m+h_out;
R_nh_out=R_n+h_out;

mpv=[0,1/R_mh_out,0;sec(L_out)/R_nh_out,0,0;0,0,1];

%速度均值
v_ave=(v_new+v_before)/2;

%位置更新方程
p_new=p_before+mpv*v_ave*ts;
end